// main.cpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include "search_algorithm.hpp"
#include "bfs_algorithm.hpp"
#include "astart_algorithm.hpp"
#include "ap_theta_algorithm.hpp"

const int occ_size = 11;

void markObstacles(cv::Mat &image, const std::vector<std::vector<int>> &grid)
{
  for (std::vector<std::vector<int>>::size_type y = 0; y < grid.size(); ++y)
  {
    for (std::vector<std::vector<int>>::size_type x = 0; x < grid[0].size(); ++x)
    {
      if (grid[y][x] == 1)
      {
        cv::Rect roi(x * occ_size, y * occ_size, occ_size, occ_size);
        image(roi) = cv::Vec3b(0, 0, 0);
      }
    }
  }
}

void markStart(cv::Mat &image, Point point)
{
  cv::Rect roi(point.x * occ_size, point.y * occ_size, occ_size, occ_size);
  image(roi) = cv::Vec3b(255, 0, 0);
}

void markGoal(cv::Mat &image, Point point)
{
  cv::Rect roi(point.x * occ_size, point.y * occ_size, occ_size, occ_size);
  image(roi) = cv::Vec3b(0, 0, 125);
}

int main()
{
  std::string pic = "/home/icebot/Nav2_demo/Image/6.png";
  Point start = {4 * occ_size, 5 * occ_size};
  Point end = {7 * occ_size, 5 * occ_size};

  cv::Mat recive_im = cv::imread(pic);

  if (recive_im.empty())
  {
    std::cerr << "无法读取图像文件" << std::endl;
    return -1;
  }

  cv::Size new_size(occ_size * 10, occ_size * 10);
  cv::resize(recive_im, recive_im, new_size, 0, 0, cv::INTER_LINEAR);

  std::vector<std::vector<int>> grid_map;

  for (int y = 0; y < new_size.height; ++y)
  {
    std::vector<int> row;
    for (int x = 0; x < new_size.width; ++x)
    {
      cv::Vec3b pixel = recive_im.at<cv::Vec3b>(y, x);
      if (pixel == cv::Vec3b(0, 0, 0))
      {
        row.emplace_back(1);
      }
      else
      {
        row.emplace_back(0);
      }
    }
    grid_map.emplace_back(row);
  }

  for (const auto &row : grid_map)
  {
    for (int value : row)
    {
      std::cout << value << " ";
    }
    std::cout << std::endl;
  }

  cv::Mat output_image(grid_map.size() * occ_size, grid_map[0].size() * occ_size, CV_8UC3, cv::Scalar(255, 255, 255));
  markStart(output_image, start);
  markGoal(output_image, end);
  markObstacles(output_image, grid_map);

  SearchAlgorithm *algorithm = new APThetaAlgorithm();
  // 可以选择不同算法
  // SearchAlgorithm *algorithm = new BFSAlgorithm();
  // SearchAlgorithm *algorithm = new AStarAlgorithm();

  std::vector<Point> path = algorithm->search(grid_map, start, end, output_image, occ_size);

  if (path.empty())
  {
    std::cout << "未找到路径" << std::endl;
  }
  else
  {
    std::cout << "找到路径，路径长度: " << path.size() << std::endl;
    for (size_t i = 1; i < path.size() - 1; ++i)
    {
      std::cout << "(" << path[i].x << ", " << path[i].y << ") -> ";
      cv::Rect roi(path[i].x * occ_size, path[i].y * occ_size, occ_size, occ_size);
      output_image(roi) = cv::Vec3b(0, 0, 255);
    }
    std::cout << std::endl;
  }

  cv::imshow("Expansion with Path", output_image);
  cv::waitKey(0);

  delete algorithm;
  return 0;
}
